/*
 * @Description: 后端具体实现
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:05:52
 * @LastEditTime: 2021-10-27 14:03:56
 * @LastEditors: Sang Hao
 */

#ifndef LIDAR_SLAM_MAPPING_BACK_END_BACK_END_HPP_
#define LIDAR_SLAM_MAPPING_BACK_END_BACK_END_HPP_

#include <string>
#include <fstream>
#include <deque>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>

#include "lidar_slam/sensor_data/key_frame.hpp"
#include "lidar_slam/sensor_data/cloud_data.hpp"
#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/sensor_data/pose_data.hpp"
#include "lidar_slam/sensor_data/loop_pose.hpp"

#include "lidar_slam/models/graph_optimizer/g2o/g2o_graph_optimizer.hpp"

namespace lidar_slam {
class BackEnd {
private:
	/* data */
	std::string key_frames_path_ = "";
	std::string trajectory_path_ = "";

	std::ofstream ground_truth_ofs_;
	std::ofstream laser_odom_ofs_;
	std::ofstream optimized_pose_ofs_;

	float key_frame_distance_ = 2.0;

	bool has_new_key_frame_ = false;
	bool has_new_optimized_ = false;

	KeyFrame current_key_frame_;
	std::deque<KeyFrame> key_frames_deque_;
	std::deque<Eigen::Matrix4f> optimized_pose_;

	/* 优化器 */
	std::shared_ptr<InterfaceGraphOptimizer> graph_optimizer_ptr_;

	class GraphOptimizerConfig {
	public:
		GraphOptimizerConfig() {
			odom_edge_noise.resize(6);
			close_loop_noise.resize(6);
		}
	public:
		bool use_loop_close = false;

		Eigen::VectorXd odom_edge_noise;
		Eigen::VectorXd close_loop_noise;

		int optimize_step_with_key_frame = 100;
		int optimize_step_with_loop = 10;
	};

	GraphOptimizerConfig graph_optimizer_config_;

	int new_loop_cnt_ = 0;
	int new_key_frame_cnt_ = 0;	

private:
	bool InitWithConfig();
	bool InitParam(const YAML::Node& config_node);
	bool InitGraphOptimizer(const YAML::Node& config_node);
	bool InitDataPath(const YAML::Node& config_node);

	void ResetParam();
	bool SavePose(std::ofstream& ofs, const Eigen::Matrix4f& pose);
	bool AddNodeAndEdge();
	bool MaybeNewKeyFrame(const CloudData& cloud_data, const PoseData& laser_odom);
	bool MaybeOptimized();
	bool SaveOptimizedPose();


public:
	BackEnd(/* args */);

	bool Update(const CloudData& cloud_data, const PoseData& laser_odom);
	bool InsertLoopPose(const LoopPose& loop_pose);
	bool ForceOptimize();

	void GetOptimizedKeyFrames(std::deque<KeyFrame>& key_frames_deque);
	bool HasNewKeyFrame();
	bool HasNewOptimized();
	void GetLatestKeyFrame(KeyFrame& key_frame);
	// void GetLatestKeyGNSS(KeyFrame& key_frame);
	
};
}

#endif